function q = rotmat2quat(m)

m00 = m(1, 1);
m11 = m(2, 2);
m22 = m(3, 3);

tr = m00 + m11 + m22 + 1;

if (tr > 0.00000001)
    s = 0.5 / sqrt(tr);
    w = 0.25 / s;

    x = (m(3, 2) - m(2, 3)) * s;
    y = (m(1, 3) - m(3, 1)) * s;
    z = (m(2, 1) - m(1, 2)) * s;
else
    i = 0;
    if (m11 > m00) 
        i = 1;
    end
    if (m22 > m(i+1, i+1)) 
        i = 2;
    end

    j = mod(i + 1, 3);
    k = mod(j + 1, 3);

    s = sqrt(m(i+1, i+1) - m(j+1, j+1) + m(k+1, k+1) + 1);

    q(i+1) = s * 0.5;
    if (fabs(s) > 0.00000001) 
        s = 0.5 / s;
    end

    q(3+1) = (m(j+1, k+1) - m(k+1, j+1)) * s;
    q(j+1) = (m(i+1, j+1) + m(j+1, i+1)) * s;
    q(k+1) = (m(i+1, k+1) + m(k+1, i+1)) * s;

     w = q(3+1); x = q(0+1); y = q(1+1); z = q(2+1);
end

q = [x y z w];
q = q./sqrt(sum(q.*q));
